#!coding:utf-8
import numpy as np
import time
from pupper.config import Command, State, BehaviorState, Configuration
from pupper.controller import Controller
from pupper.inverse_kinematics import four_legs_inverse_kinematics
from pupper.hardware_interface import HardwareInterface
from pupper.joystick_interface import JoystickInterface


def main():
    """
    主函数
    """

    # 初始化各种对象
    config = Configuration()
    controller = Controller(
        config,
    )
    joystick = JoystickInterface(config)
    state = State()
    command = Command()

    hardware_interface = HardwareInterface()

    # 初始姿态
    state.foot_locations = (
        config.default_stance
        + np.array([0, 0, command.height])[:, np.newaxis]
    )
    last_loop = time.time()

    print("Summary of gait parameters:")
    print("overlap time: ", config.overlap_time)
    print("swing time: ", config.swing_time)
    print("z clearance: ", config.z_clearance)
    print("x shift: ", config.x_shift)

    hardware_interface.slow_up()

    while True:
        now = time.time()
        # 每过一个时钟周期才进行一次计算
        if now - last_loop < config.dt:
            continue
        last_loop = time.time()

        command = joystick.get_command(state)

        # 执行一次坐标计算
        controller.run(state, command)

        if state.behavior_state != BehaviorState.DEACTIVATED:
            # 打印计算出来的坐标
            print(state.rotated_foot_locations)

            # 坐标到关节角度
            joint_angles = four_legs_inverse_kinematics(
                state.rotated_foot_locations, config)

            # 关节角度到舵机指令
            hardware_interface.set_actuator_postions(joint_angles)


if __name__ == '__main__':
    main()
